package Strategy;

import Observer.EnvironmentData;
import java.util.ArrayList;
import java.util.List;

/**
 * 环境自适应路径规划策略 - 根据环境数据动态调整路径
 */
public class EnvironmentAdaptivePathPlanning implements PathPlanningStrategy {

    private EnvironmentData environmentData;

    public EnvironmentAdaptivePathPlanning(EnvironmentData data) {
        this.environmentData = data;
    }

    @Override
    public Path calculatePath(FieldMap map, EnvironmentData data) {
        List<Coordinate> waypoints = new ArrayList<>();
        double fieldWidth = map.getWidth();
        double fieldHeight = map.getHeight();
        double machineWidth = 3.0;
        double spacing = machineWidth * 0.9;

        // 根据土壤湿度调整路径
        if (data.getSoilMoisture() > 0.7) {
            // 湿度过高，减少对土壤的压实，采用更大的行距
            spacing = machineWidth * 1.2;
        } else if (data.getSoilMoisture() < 0.3) {
            // 湿度不足，增加覆盖密度
            spacing = machineWidth * 0.8;
        }

        // 根据温度调整路径
        if (data.getTemperature() > 35) {
            // 高温环境，优先处理阴凉区域
            for (double y = fieldHeight/2; y < fieldHeight; y += spacing) {
                waypoints.add(new Coordinate(0, y));
                waypoints.add(new Coordinate(fieldWidth, y));
            }
            for (double y = 0; y < fieldHeight/2; y += spacing) {
                waypoints.add(new Coordinate(fieldWidth, y));
                waypoints.add(new Coordinate(0, y));
            }
        } else {
            // 常规蛇形路径
            for (double y = 0; y < fieldHeight; y += spacing) {
                if ((int)(y/spacing) % 2 == 0) {
                    waypoints.add(new Coordinate(0, y));
                    waypoints.add(new Coordinate(fieldWidth, y));
                } else {
                    waypoints.add(new Coordinate(fieldWidth, y));
                    waypoints.add(new Coordinate(0, y));
                }
            }
        }

        // 避开障碍物
        for (Coordinate obstacle : map.getObstaclePositions()) {
            waypoints = avoidObstacle(waypoints, obstacle, 5.0); // 障碍物周围5米绕行
        }

        return new Path(waypoints, "Environment Adaptive Path");
    }

    private List<Coordinate> avoidObstacle(List<Coordinate> originalPath, Coordinate obstacle, double safetyDistance) {
        List<Coordinate> newPath = new ArrayList<>();

        for (int i = 0; i < originalPath.size() - 1; i++) {
            Coordinate start = originalPath.get(i);
            Coordinate end = originalPath.get(i + 1);

            // 检查线段是否接近障碍物
            if (isSegmentCloseToObstacle(start, end, obstacle, safetyDistance)) {
                // 计算绕行路径
                List<Coordinate> detour = calculateDetour(start, end, obstacle, safetyDistance);
                newPath.addAll(detour);
            } else {
                newPath.add(start);
            }
        }

        // 添加最后一个点
        if (!originalPath.isEmpty()) {
            newPath.add(originalPath.get(originalPath.size() - 1));
        }

        return newPath;
    }

    private boolean isSegmentCloseToObstacle(Coordinate start, Coordinate end, Coordinate obstacle, double threshold) {
        // 简化的距离计算，实际应使用点到线段的距离公式
        double midX = (start.getX() + end.getX()) / 2;
        double midY = (start.getY() + end.getY()) / 2;
        Coordinate midPoint = new Coordinate(midX, midY);

        return distance(midPoint, obstacle) < threshold;
    }

    private double distance(Coordinate p1, Coordinate p2) {
        return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) +
                Math.pow(p1.getY() - p2.getY(), 2));
    }

    private List<Coordinate> calculateDetour(Coordinate start, Coordinate end, Coordinate obstacle, double distance) {
        List<Coordinate> detour = new ArrayList<>();

        // 简单的绕行策略 - 从起点向垂直方向偏移，绕过障碍物后再回到原路线
        double directionX = end.getX() - start.getX();
        double directionY = end.getY() - start.getY();
        double length = Math.sqrt(directionX * directionX + directionY * directionY);

        // 归一化方向向量
        directionX /= length;
        directionY /= length;

        // 计算垂直方向
        double perpendicularX = -directionY;
        double perpendicularY = directionX;

        // 确定绕行方向（障碍物在左侧还是右侧）
        double crossProduct = (obstacle.getX() - start.getX()) * directionY -
                (obstacle.getY() - start.getY()) * directionX;

        if (crossProduct > 0) {
            // 障碍物在路径左侧，向右绕行
            perpendicularX = -perpendicularX;
            perpendicularY = -perpendicularY;
        }

        // 计算绕行点
        Coordinate detourStart = new Coordinate(
                start.getX() + perpendicularX * distance,
                start.getY() + perpendicularY * distance
        );

        Coordinate detourEnd = new Coordinate(
                end.getX() + perpendicularX * distance,
                end.getY() + perpendicularY * distance
        );

        detour.add(start);
        detour.add(detourStart);
        detour.add(detourEnd);
        detour.add(end);

        return detour;
    }
}